from machine import I2C
from micropython import const
import utime


# MS5611 device commands
_MS5611_RESET_COMMAND 						= const(0x1E)
_MS5611_START_PRESSURE_ADC_CONVERSION		= const(0x40)
_MS5611_START_TEMPERATURE_ADC_CONVERSION	= const(0x50)
_MS5611_READ_ADC							= const(0x00)

_MS5611_CMD_ADC_256							= const(0x00) # ADC OSR=256
_MS5611_CMD_ADC_512							= const(0x02) # ADC OSR=512
_MS5611_CMD_ADC_1024						= const(0x04) # ADC OSR=1024
_MS5611_CMD_ADC_2048						= const(0x06) # ADC OSR=2048
_MS5611_CMD_ADC_4096						= const(0x08) # ADC OSR=4096

_MS5611_CONVERSION_OSR_MASK					= const(0x0F)

_MS5611_CONVERSION_TIME_OSR_256 = const(1000)
_MS5611_CONVERSION_TIME_OSR_512 = const(2000)
_MS5611_CONVERSION_TIME_OSR_1024 = const(3000)
_MS5611_CONVERSION_TIME_OSR_2048 = const(5000)
_MS5611_CONVERSION_TIME_OSR_4096 = const(9000)

# MS5611 commands
_MS5611_PROM_ADDRESS_READ_ADDRESS_0 = const(0xA0)
_MS5611_PROM_ADDRESS_READ_ADDRESS_1 = const(0xA2)
_MS5611_PROM_ADDRESS_READ_ADDRESS_2 = const(0xA4)
_MS5611_PROM_ADDRESS_READ_ADDRESS_3 = const(0xA6)
_MS5611_PROM_ADDRESS_READ_ADDRESS_4 = const(0xA8)
_MS5611_PROM_ADDRESS_READ_ADDRESS_5 = const(0xAA)
_MS5611_PROM_ADDRESS_READ_ADDRESS_6 = const(0xAC)
_MS5611_PROM_ADDRESS_READ_ADDRESS_7 = const(0xAE)

# Coefficients indexes for temperature and pressure computation
# _MS5611_CRC_INDEX											7
# _MS5611_PRESSURE_SENSITIVITY_INDEX							1 
# _MS5611_PRESSURE_OFFSET_INDEX								2
# _MS5611_TEMP_COEFF_OF_PRESSURE_SENSITIVITY_INDEX				3
# _MS5611_TEMP_COEFF_OF_PRESSURE_OFFSET_INDEX					4
# _MS5611_REFERENCE_TEMPERATURE_INDEX							5
# _MS5611_TEMP_COEFF_OF_TEMPERATURE_INDEX						6
# _MS5611_COEFFICIENT_NUMBERS									8

class MS5611:
	def __init__(self, i2c, addr=0x77):
		if i2c is None or i2c.__class__ != I2C:
			raise ValueError("I2C object needed as argument!")
		self._i2c = i2c
		self._addr = addr

		self._i2c.writeto(self._addr, bytes([_MS5611_RESET_COMMAND]))
		utime.sleep_ms(3)		# need wait about 2.8ms after reset
		self._read_eeprom()

	def _read_eeprom(self):
		# Read 12 bytes of calibration data
		# Read pressure sensitivity
		data = self._i2c.readfrom_mem(self._addr, _MS5611_PROM_ADDRESS_READ_ADDRESS_1, 2)
		self.C1 = (data[0] << 8) + data[1]

		# Read pressure offset
		data = self._i2c.readfrom_mem(self._addr, _MS5611_PROM_ADDRESS_READ_ADDRESS_2, 2)
		self.C2 = (data[0] << 8) + data[1]

		# Read temperature coefficient of pressure sensitivity
		data = self._i2c.readfrom_mem(self._addr, _MS5611_PROM_ADDRESS_READ_ADDRESS_3, 2)
		self.C3 = (data[0] << 8) + data[1]

		# Read temperature coefficient of pressure offset
		data = self._i2c.readfrom_mem(self._addr, _MS5611_PROM_ADDRESS_READ_ADDRESS_4, 2)
		self.C4 = (data[0] << 8) + data[1]

		# Read reference temperature
		data = self._i2c.readfrom_mem(self._addr, _MS5611_PROM_ADDRESS_READ_ADDRESS_5, 2)
		self.C5 = (data[0] << 8) + data[1]

		# Read temperature coefficient of the temperature
		data = self._i2c.readfrom_mem(self._addr, _MS5611_PROM_ADDRESS_READ_ADDRESS_6, 2)
		self.C6 = (data[0] << 8) + data[1]

		# TODO:crc4 

	def start_d1(self):
		self._i2c.writeto(self._addr, bytes([_MS5611_START_PRESSURE_ADC_CONVERSION | _MS5611_CMD_ADC_4096]), 1)

	def start_d2(self):
		self._i2c.writeto(self._addr, bytes([_MS5611_START_TEMPERATURE_ADC_CONVERSION | _MS5611_CMD_ADC_4096]), 1)

	def read_d1(self):
		data = self._i2c.readfrom_mem(self._addr, _MS5611_READ_ADC, 3)
		self.D1 = (data[0] << 16) + (data[1] << 8) + data[2]

	def read_d2(self):
		data = self._i2c.readfrom_mem(self._addr, _MS5611_READ_ADC, 3)
		self.D2 = (data[0] << 16) + (data[1] << 8) + data[2]

	def read_pressure(self):
		dT = self.D2 - (self.C5 << 8)
		TEMP = 2000 + (dT * self.C6 / 8388608)
		OFF = (self.C2 << 16) + (self.C4 * dT) / 128
		SENS = (self.C1 << 15) + (self.C3 * dT ) / 256
		T2 = 0
		OFF2 = 0
		SENS2 = 0

		if TEMP >= 2000 :
			T2 = 0
			OFF2 = 0
			SENS2 = 0
		elif TEMP < 2000 :
			T2 = (dT * dT) / 2147483648
			OFF2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 2
			SENS2 = 5 * ((TEMP - 2000) * (TEMP - 2000)) / 4
			if TEMP < -1500 :
				OFF2 = OFF2 + 7 * ((TEMP + 1500) * (TEMP + 1500))
				SENS2 = SENS2 + 11 * ((TEMP + 1500) * (TEMP + 1500)) / 2

		TEMP = TEMP - T2
		OFF = OFF - OFF2
		SENS = SENS - SENS2
		pressure = ((((self.D1 * SENS) / 2097152) - OFF) / 32768.0) / 100.0
		cTemp = TEMP / 100.0
		return pressure, cTemp
